/** 
 * @file Magnetometer.c
 * 
 * @detail Description : 
 * Magnetometer function calls
 * 
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
 * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 **/

/* ******************************************************* */
/* I2C code for HMC5583 magnetometer                       */
/*                                                         */
/* ******************************************************* */

#include "Magnetometer.h"

/**
* @brief <b>Function Name</b>:     : Init_Compass                                               
* @brief  <b>Description</b>: This function initializes the magnetometer, by 
* enabling the continous-measurement mode and setting the gain configuration to
* +- .88 Ga ( Gain value = 0). Also, it calculates the scaled values for each 
* axes with calibrate().
* as the clock reference.
* @param Input Parameters: None
* @return Return Values: None
**/ 
void Init_Compass()
{
  // Magnetometer
  I2C_Set_Slave_Address(HMC58X3_ADDR);
  Sleep(20);
  
  /*************************** Initialization *********************************/
  global_tx_buffer[0] = HMC58X3_R_CONFA;	// Register
  global_tx_buffer[1] = 0x70; 	              // 8 samples averaged per measurement
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  
  global_tx_buffer[0] = HMC58X3_R_CONFB;	// Register
  global_tx_buffer[1] = 0xA0; 	              // Gain configurated to +-4.7 Ga
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  
  global_tx_buffer[0] = HMC58X3_R_MODE;	// Register
  global_tx_buffer[1] = 0x00; 	        // Continuous-measurement mode enabled
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  
  // Calibrate HMC using self test, not recommended to change the gain after calibration.
  calibrate(0); // Use gain 1=default, valid 0-7, 7 not recommended.
  
  // Single mode conversion was used in calibration, now set continuous mode
  global_tx_buffer[0] = HMC58X3_R_MODE; // Register
  global_tx_buffer[1] = 0x00; // Set Mode to 0x00
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
}

/**
* @brief <b>Function Name</b>:     : Read_Compass                                               
* @brief  <b>Description</b>: This function calls getRaw(), to read the magnetometer's 
* raw x, y, and z value, stores it in the sen_data structure. Afterwards it 
* stores the float values in the sen_data structure by dividing the raw value by
* the scale values for each axes. The offset for each axes is then subtracted from
* the scaled raw values. Soft calibration is applied to the hard calibrated values
* and the heading of the AHRS board is calculated calling Compass_Heading(). 
* @param Input Parameters: None
* @return Return Values: None
**/ 
void Read_Compass(void)
{
    
          float magRot_x;
          float magRot_y;
          float magRot_z;          
          
          
          getRaw(&sen_data.magnetom_x_raw,&sen_data.magnetom_y_raw,&sen_data.magnetom_z_raw);
          
          // Scale Raw data
          sen_data.magnetom_x = ((float) sen_data.magnetom_x_raw) / x_scale;
          sen_data.magnetom_y = ((float) sen_data.magnetom_y_raw) / y_scale;
          sen_data.magnetom_z = ((float) sen_data.magnetom_z_raw) / z_scale;

          //===============================================
          // Apply Hard iron compensation 
          // This removes effects of permanetly magnetic material
          // that is on the sensor platform
          // (offset X,Y,and Z of raw data)
          
          sen_data.magnetom_x -= sen_offset.magnetom_offset[0];
          sen_data.magnetom_y -= sen_offset.magnetom_offset[1];
          sen_data.magnetom_z -= sen_offset.magnetom_offset[2];
          
          
          
          //===============================================
          // Apply Soft iron compensation
          // This removes the distortion caused by ferrous 
          // material that is on the sensor platform
          // (turn the elipse into a circle)
   
          //===============================================
          //Apply correction to the X axis
          
          //rotate the XY vector
          magRot_x = sen_data.magnetom_x * cos(sen_offset.magnetom_XY_Theta) + sen_data.magnetom_y *sin(sen_offset.magnetom_XY_Theta);
          magRot_y = sen_data.magnetom_y * cos(sen_offset.magnetom_XY_Theta) - sen_data.magnetom_x *sin(sen_offset.magnetom_XY_Theta);
          
          //Scale the X
          sen_data.magnetom_x = magRot_x/sen_offset.magnetom_XY_Scale;
          
          //rotate the XY vector back
          sen_data.magnetom_x = sen_data.magnetom_x * cos(-sen_offset.magnetom_XY_Theta) + magRot_y * sin(-sen_offset.magnetom_XY_Theta);
          sen_data.magnetom_y = magRot_y * cos(-sen_offset.magnetom_XY_Theta) + sen_data.magnetom_x * sin(-sen_offset.magnetom_XY_Theta);
          
          
          
          //===============================================
          //Apply correction to the Z axis
          
          //rotate the YZ vector
          magRot_z = sen_data.magnetom_z * cos(sen_offset.magnetom_YZ_Theta) + sen_data.magnetom_y *sin(sen_offset.magnetom_YZ_Theta);
          magRot_y = sen_data.magnetom_y * cos(sen_offset.magnetom_YZ_Theta) - sen_data.magnetom_z *sin(sen_offset.magnetom_YZ_Theta);
          
          //Scale the Z
          sen_data.magnetom_z = magRot_z/sen_offset.magnetom_YZ_Scale;
          
          //rotate the YZ vector back
          sen_data.magnetom_z = sen_data.magnetom_z * cos(-sen_offset.magnetom_YZ_Theta) + magRot_y * sin(-sen_offset.magnetom_YZ_Theta);
          sen_data.magnetom_y = magRot_y * cos(-sen_offset.magnetom_YZ_Theta) + sen_data.magnetom_z * sin(-sen_offset.magnetom_YZ_Theta);
          
          
          //===============================================
          // Now that the magnetometer data is corrected,
          // we can calculate our magnetic heading
          Compass_Heading(); // Calculate magnetic heading 
}


/**
* @brief <b>Function Name</b>:     : Compass_Heading                                               
* @brief  <b>Description</b>: This function calculates the heading of the AHRS
* board using the hard and soft calibrated magnetometer values. Then stores the
* result in the sen_data structure. 
* @param Input Parameters: None
* @return Return Values: None
**/ 
void Compass_Heading(void)
{
  float MAG_X;
  float MAG_Y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  
  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch);
  
  // Tilt compensated Magnetic field X:
  MAG_X = sen_data.magnetom_x*cos_pitch + sen_data.magnetom_y*sin_roll*sin_pitch + sen_data.magnetom_z*cos_roll*sin_pitch;
  
  // Tilt compensated Magnetic field Y:
  MAG_Y = sen_data.magnetom_y*cos_roll - sen_data.magnetom_z*sin_roll;
  
  // Magnetic Heading
  sen_data.magnetom_heading = atan2(-1*MAG_Y,MAG_X);
  
  if(sen_data.magnetom_heading < 0) 
    sen_data.magnetom_heading += 2 * M_PI;
 
}
